extends Spatial

export var tick_duration = 1.0

export var MAX_AXIS_SPEED = 1.0

var scl = 100.0

onready var XXX = find_node("XXX")
onready var YYY = find_node("YYY")
onready var ZZZ = find_node("ZZZ")


var INIT_X = 1.0
onready var INIT_Y = 1.0 - find_node("EXTRUDER").scale.y / 2.0
var INIT_Z = -1.0

var cur_line = 0
var maxlen = 0

var TEXT

onready var MONITOR = find_parent("MAIN").find_node("GCODE_MONITOR")
onready var VOLUME = find_parent("MAIN").find_node("VOLUME")


#var x = 0.0
#var y = 1.0
#var z = 0.0

func _ready():

	var text = load("res://calibrator.gcode.gd").new()
	TEXT = text.text
	maxlen = len(TEXT)
	calibrate_thing()
	go_to_pos(0.0,0.0,0.0)


func calibrate_thing():
	var max_vector = Vector3(-1,-1,-1)
	var min_vector = Vector3 (1000,1000,1000) #assumption: no more real printer than 1mx1mx1m

	var CSG_MESH = find_node("MESH")
	var mesh = find_node("MESH").mesh

	for line in TEXT:
		if line.begins_with('G1'):
			var tokens = line.split(' ')
			for t in tokens:

				if t.begins_with('X'):
					var value = float(t.right(1))
					max_vector.x = max(max_vector.x,value)
					min_vector.x = min(min_vector.x,value)

				elif t.begins_with('Y'): #gcode has different coordinate system!
					var value = float(t.right(1))
					max_vector.z = max(max_vector.z,value)
					min_vector.z = min(min_vector.z,value)

				elif t.begins_with('Z'):
					var value = float(t.right(1))
					max_vector.y = max(max_vector.y,value)
					min_vector.y = min(min_vector.y,value)


	max_vector /= scl
	min_vector /= scl

	print ("min_vector ", min_vector)
	print ("max_vector ", max_vector)

	var max_m_vector = Vector3(-1,-1,-1)
	var min_m_vector = Vector3 (1000,1000,1000)

	for f in mesh.get_faces():
		max_m_vector.x = max(max_m_vector.x,f.x)
		min_m_vector.x = min(min_m_vector.x,f.x)

		max_m_vector.y = max(max_m_vector.y,f.y)
		min_m_vector.y = min(min_m_vector.y,f.y)

		max_m_vector.z = max(max_m_vector.z,f.z)
		min_m_vector.z = min(min_m_vector.z,f.z)

	max_m_vector /= scl
	min_m_vector /= scl

	print ("min_m_vector ", min_m_vector)
	print ("max_m_vector ", max_m_vector)

	var span = max_vector - min_vector
	print("span = ", span)


	var span_m = max_m_vector - min_m_vector
	print("mesh span = ", span_m)

	CSG_MESH.translation -= Vector3(INIT_X,0,INIT_Z) #making mesh and gcode common reference
	CSG_MESH.translation += Vector3(min_vector.x,min_vector.y,-min_vector.z) - Vector3(min_m_vector.x,min_m_vector.y,max_m_vector.z)



func on_step_finish():
	print("done.calling next step")

	cur_line += 1
	if cur_line == maxlen:
		finish()

	var line = TEXT[cur_line]
	print(line)
	MONITOR.add_message(line)

	if line.begins_with('G1'):
#		print ('valid')
		var p = g1_2_pos(line)
		print ('translated to', p)
		print ('while now we are at', get_coords_workspace())
		go_to_pos(p.x,p.y,p.z)


	elif line.begins_with('G28'):
		print ('resetting position')
		go_to_pos(0.0,0.0,0.0)
	else:
		print ('invalid, skipping')
		on_step_finish()

func get_coords():
	return Vector3(XXX.translation.x,YYY.translation.y,ZZZ.translation.z)

func get_coords_workspace():
	return get_coords() + Vector3(INIT_X,INIT_Y,INIT_Z)

func g1_2_pos(string):
	var ret = get_coords_workspace() * scl
	var tokens = string.split(' ')
	for t in tokens:
		if t.begins_with('X'):
			ret.x = float(t.right(1))
		elif t.begins_with('Y'):
			ret.z = -float(t.right(1)) #gcode has different coordinate system!
		elif t.begins_with('Z'):
			ret.y = float(t.right(1))
	ret *= (1/scl)
	return ret

func finish():
	print ('all done!')

func go_to_pos(x,y,z):

	var movement_vector = (Vector3(x,y,z) - Vector3(INIT_X,INIT_Y,INIT_Z))-get_coords()
	print ("movement_vector = ", movement_vector)
	var abs_mv = movement_vector.abs()

	var biggest_axis = max(abs_mv.x, abs_mv.y)
	biggest_axis = max(biggest_axis,abs_mv.z)

	var anitime = float(biggest_axis) / MAX_AXIS_SPEED

	if abs_mv.y!=0:
		VOLUME.height = 2*y

	if anitime == 0:
		anitime = 0.03

	print("anitime is ", anitime)

	var step_timer = SDTimer.new(self,'on_step_finish')
	step_timer.wait_time = anitime
	add_child(step_timer)
	step_timer.start()

	var twx = SDTween.new()
	twx.interpolate_property(XXX,'translation',XXX.translation,Vector3(x-INIT_X,0,0),anitime,Tween.TRANS_LINEAR,Tween.EASE_IN_OUT)
	add_child(twx)
	twx.start()

	var twy = SDTween.new()
	twy.interpolate_property(YYY,'translation',YYY.translation,Vector3(0,y-INIT_Y,0),anitime,Tween.TRANS_LINEAR,Tween.EASE_IN_OUT)
	add_child(twy)
	twy.start()

	var twz = SDTween.new()
	twz.interpolate_property(ZZZ,'translation',ZZZ.translation,Vector3(0,0,z-INIT_Z),anitime,Tween.TRANS_LINEAR,Tween.EASE_IN_OUT)
	add_child(twz)
	twz.start()

